# To use Inference Engine backend, specify location of plugins:
# export LD_LIBRARY_PATH=/opt/intel/deeplearning_deploymenttoolkit/deployment_tools/external/mklml_lnx/lib:$LD_LIBRARY_PATH
import cv2
import numpy as np
import argparse
import math
from funtion import vector_centerpoint
from funtion import similarity
from funtion import video_brodcast

# #动作识别
# parser = argparse.ArgumentParser()
# parser.add_argument('--input', help='Path to image or video. Skip to capture frames from camera')
# parser.add_argument('--thr', default=0.2, type=float, help='Threshold value for pose parts heat map')
# parser.add_argument('--width', default=368, type=int, help='Resize input to specific width.')
# parser.add_argument('--height', default=368, type=int, help='Resize input to specific height.')

def Motionrecognition():

    #args = parser.parse_args()

    BODY_PARTS = { "Nose": 0, "Neck": 1, "RShoulder": 2, "RElbow": 3, "RWrist": 4,
                "LShoulder": 5, "LElbow": 6, "LWrist": 7, "RHip": 8, "RKnee": 9,
                "RAnkle": 10, "LHip": 11, "LKnee": 12, "LAnkle": 13, "REye": 14,
                "LEye": 15, "REar": 16, "LEar": 17, "Background": 18 }

    POSE_PAIRS = [ ["Neck", "RShoulder"], ["Neck", "LShoulder"], ["RShoulder", "RElbow"],
                ["RElbow", "RWrist"], ["LShoulder", "LElbow"], ["LElbow", "LWrist"],
                ["Neck", "RHip"], ["RHip", "RKnee"], ["RKnee", "RAnkle"], ["Neck", "LHip"],
                ["LHip", "LKnee"], ["LKnee", "LAnkle"], ["Neck", "Nose"], ["Nose", "REye"],
                ["REye", "REar"], ["Nose", "LEye"], ["LEye", "LEar"] ]

    inWidth = 368#args.width
    inHeight = 368#args.height

    net = cv2.dnn.readNetFromTensorflow("graph_opt.pb")

    cap = cv2.VideoCapture(0)

    while True:
        x=cv2.waitKey(1)
        if x&0xFF == ord('l'):
            break
        if x&0xFF == ord('q'):
            break
        hasFrame, frame = cap.read()
        if not hasFrame:
            cv2.waitKey()
            break

        frameWidth = frame.shape[1]
        frameHeight = frame.shape[0]

        net.setInput(cv2.dnn.blobFromImage(frame, 1.0, (inWidth, inHeight), (127.5, 127.5, 127.5), swapRB=True, crop=False))
        out = net.forward()
        out = out[:, :19, :, :]  # MobileNet output [1, 57, -1, -1], we only need the first 19 elements

        assert(len(BODY_PARTS) == out.shape[1])

        points = []
        cameraset = []
        for i in range(len(BODY_PARTS)):
            # Slice heatmap of corresponging body's part.
            heatMap = out[0, i, :, :]
            #print(heatMap)

            # Originally, we try to find all the local maximums. To simplify a sample
            # we just find a global one. However only a single pose at the same time
            # could be detected this way.
            _, conf, _, point = cv2.minMaxLoc(heatMap)
            x = (frameWidth * point[0]) / out.shape[3]
            cameraset.append(x)
            y = (frameHeight * point[1]) / out.shape[2]
            cameraset.append(y)
            
            # Add a point if it's confidence is higher than threshold.
            points.append((int(x), int(y)) if conf >  0.2 else None)

            #args.thr
        image = np.array([293.65217391, 106.7826087 , 293.65217391, 173.52173913 ,240.26086957,
            186.86956522 ,240.26086957 ,293.65217391, 253.60869565 ,360.39130435,
            333.69565217, 173.52173913, 360.39130435, 280.30434783, 320.34782609,
            106.7826087 , 266.95652174 ,360.39130435 ,240.26086957 ,427.13043478,
            253.60869565, 440.47826087 ,333.69565217, 347.04347826 ,347.04347826,
            427.13043478 ,253.60869565 ,440.47826087, 280.30434783 ,106.7826087 ,
            293.65217391 , 93.43478261 ,280.30434783 ,106.7826087 , 320.34782609,
            106.7826087  ,587.30434783 , 13.34782609])
        camera = np.array(cameraset) #一维数组转矩阵行向量
        #print(camera)
        vector_centerpoint(image)
        vector_centerpoint(camera)
        #print(vector_centerpoint(camera))
        #print(similarity(image,camera))
        video_brodcast(image,camera)

        for pair in POSE_PAIRS:
            partFrom = pair[0]
            partTo = pair[1]
            assert(partFrom in BODY_PARTS)
            assert(partTo in BODY_PARTS)

            idFrom = BODY_PARTS[partFrom]
            idTo = BODY_PARTS[partTo]

            if points[idFrom] and points[idTo]:
                cv2.line(frame, points[idFrom], points[idTo], (0, 255, 0), 3)
                cv2.ellipse(frame, points[idFrom], (3, 3), 0, 0, 360, (0, 0, 255), cv2.FILLED)
                cv2.ellipse(frame, points[idTo], (3, 3), 0, 0, 360, (0, 0, 255), cv2.FILLED)

        t, _ = net.getPerfProfile()
        freq = cv2.getTickFrequency() / 1000
        cv2.putText(frame, '%.2fms' % (t / freq), (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0))

        cv2.imshow('Test', frame)  
    return x

#-----------------------------分割线----------------------------
'''image = np.array([293.65217391, 106.7826087 , 293.65217391, 173.52173913 ,240.26086957,
 186.86956522 ,240.26086957 ,293.65217391, 253.60869565 ,360.39130435,
 333.69565217, 173.52173913, 360.39130435, 280.30434783, 320.34782609,
 106.7826087 , 266.95652174 ,360.39130435 ,240.26086957 ,427.13043478,
 253.60869565, 440.47826087 ,333.69565217, 347.04347826 ,347.04347826,
 427.13043478 ,253.60869565 ,440.47826087, 280.30434783 ,106.7826087 ,
 293.65217391 , 93.43478261 ,280.30434783 ,106.7826087 , 320.34782609,
 106.7826087  ,587.30434783 , 13.34782609])
camera = np.array(cameraset) #一维数组转矩阵行向量
#print(camera)
vector_centerpoint(image)
vector_centerpoint(camera)
#print(similarity(image,camera))
video_brodcast(image,camera)'''
